// --------------------------------------------------------------------------------------------------------------------
// <copyright file="Ev4FloorDataPruner.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Cameras
{
    using System;
    using System.Collections.Generic;
    using System.Drawing;
    using System.Runtime.CompilerServices;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Core.Algorithms;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Interface for flat surface learn data pruning. Specific implementations of it may prune floor or walls, etc. We have
    /// one implementation of it at the moment - Ev4FloorDataPruner.
    /// Instances of this type are expected to prune strip learn data and return clean candidates for fitting planes.
    /// Actual outlier pruning and validation techniques are not codified by this interface. Implementations
    /// may choose to get as sophisticated as they need to, depending on the desired degree of robustness.
    /// The client of this interface is Expected to:
    ///   1. Set the calibration data (once per lifetime of a pruner instance)
    ///   2. Call PrepareForNewPruningSession 
    ///   3. Add raw strip learn data (A,B,Variance for each surface strip)
    ///   4. Call PruneOutliers (to get final pruned set of A's, and B's which look good individually)
    ///   5. Call IsLearnDataValid on pruned set and commit to learned data if it returns true. otherwise - 
    ///     stick to old learn data, as new data was not deemed reliable enough to apply
    /// </summary>
    public interface IFlatSurfaceLearnDataPruner
    {
        /// <summary>
        /// Initializes an object with floor filtering calibration data
        /// </summary>
        /// <param name="calibrationData">An array initialized with pre-learned A and B coefficients in rows, per strip of floor (columns)</param>
        void SetCalibrationData(double[][] calibrationData);

        /// <summary>
        /// Determines whether or not the pruner is initialized with pre-learned data
        /// </summary>
        /// <returns>Whether or not learn data has been set</returns>
        bool IsInitialized();

        /// <summary>
        /// Clears all previous state and starts a new pruning session.
        /// </summary>
        /// <param name="frameHeight">Frame height.</param>
        /// <param name="frameWidth">Frame width. </param>
        /// <param name="stripCount">Number of strips</param>
        void PrepareForNewPruningSession(int frameHeight, int frameWidth, int stripCount);

        /// <summary>
        /// Add sample to pruner
        /// </summary>
        /// <param name="stripCenter">Strip center coordinate</param>
        /// <param name="floorFitResult">Strip fit coefficients</param>
        /// <param name="averageStripDepth">Average depths</param>
        void AddSample(
            int stripCenter,
            FastTuple<double, double, double> floorFitResult,
            double averageStripDepth);

        /// <summary>
        /// Prunes outlier coefficients. We apply a series of filters to make sure that what we learn is indeed floor
        /// and not just any surface
        /// </summary>
        void PruneOutliers();

        /// <summary>
        /// Validates pruned strip data to make sure it makes sense as a whole (after pruning removes outliers)
        /// </summary>
        /// <returns>Whether or not the final A dataset of good for fitting a floor plane</returns>
        bool IsLearnDataValid();

        /// <summary>
        /// Gets pruned A and B data points
        /// </summary>
        /// <param name="dataPointsA">Pruned A data points</param>
        /// <param name="dataPointsB">Pruned B data points</param>
        /// <param name="dataPointsD">Pruned Depth data points</param>
        void GetPrunedDataPoints(out List<FastTuple<double, double>> dataPointsA, out List<FastTuple<double, double>> dataPointsB, out List<FastTuple<double, double>> dataPointsD);
    }

    /// <summary>
    /// Floor learn data pruner's job is to go through all A's, B's, and fitting Variances and throw out ones that do not belong (to a flat floor).
    /// There are two stages of pruning:
    /// 1. Go through all A's, B's and V's for each floor strip, which have been calculated externally (by the learner)
    ///   and throw out ones that do not fit in the absolute or relative margins (see PruneOutliers(...) method). On this
    ///   stage we deal with A, B and V data points one-by-one, and then join the results to get a final set of strips
    ///   for which all A, B and V parameters are good.
    /// 2. Validate the pruned set. At this stage we make judgment about the pruned data set and make a "go / no go"
    ///   decision on the entire data set. We decide to throw away the entire collection if we detect ambiguous
    ///   data in the set (i.e. two floors). At this point, we can not 'blame' one value over another, as they all 
    ///   belong to some near-floor flat surface, but we also don't want to commit to any one of them since it
    ///   may result in an incorrect guess (i.e. a large flat board lying on the floor slightly off the ground)
    /// </summary>
    public class Ev4FloorDataPruner : IFlatSurfaceLearnDataPruner
    {
        /// <summary>
        /// Determines the portion of strips that have to have good readings before updating floor profile. I.e. value of 4 would mean
        /// more than 1/4 of the strips must show good readings before we consider it good enough for learning. This is not an interesting 
        /// setting to externalize
        /// </summary>
        private const int MinimumGoodStripRatio = 4;

        /// <summary>
        /// Number of vertical strips in floor filtering data
        /// </summary>
        private int floorFilteringStripCount = 0;

        /// <summary>
        /// Floor filtering data we dynamically learn
        /// </summary>
        private double[][] learnedFloorConstants = null;

        /// <summary>
        /// Baseline B we get from calibration file
        /// </summary>
        private double calibratedBAverage = 0;

        /// <summary>
        /// We compute variance of A from calibration data to use as a safeguard against learning ambiguous scenes
        /// </summary>
        private double maxAllowedVarianceOfPrunedDepthStrips = 0;

        /// <summary>
        /// Defines how much tilt up we allow (beyond this - we wont extract floor). 
        /// This is a relative, resolution-independent value passed in during construction
        /// </summary>
        private double maxAllowedHorizonDisplacement;

        /// <summary>
        /// Defines how much tilt down we allow (beyond this - we wont extract floor)
        /// This is a relative, resolution-independent value passed in during construction
        /// </summary>
        private double minAllowedHorizonDisplacement;

        /// <summary>
        /// Max allowed floor roughness
        /// </summary>
        private double maxSurfaceRoughness = 0;

        /// <summary>
        /// Max allowed vertical shift up in percent as compared to calibration data
        /// </summary>
        private double maxVerticalTravelPercentFromCalibrated = 0;

        /// <summary>
        /// Max allowed vertical shift down in percent as compared to calibration data
        /// </summary>
        private double minVerticalTravelPercentFromCalibrated = 0;

        /// <summary>
        /// Pre-computed from maxVerticalVariancePercent for performance
        /// </summary>
        private double maxAllowedAbsoluteB = 0;

        /// <summary>
        /// Pre-computed from maxVerticalVariancePercent for performance
        /// </summary>
        private double minAllowedAbsoluteB = 0;

        /// <summary>
        /// Max allowed horizon pixel pre-calculated during initialization
        /// </summary>
        private double maxAllowedHorizonPixel = 0;

        /// <summary>
        /// Min allowed horizon pixel pre-calculated during initialization
        /// </summary>
        private double minAllowedHorizonPixel = 0;

        /// <summary>
        /// A data points
        /// </summary>
        private PolynomialEstimator solverA;

        /// <summary>
        /// B data points
        /// </summary>
        private PolynomialEstimator solverB;
        
        /// <summary>
        /// Variance data points
        /// </summary>
        private PolynomialEstimator solverV;

        /// <summary>
        /// Average depths data points
        /// </summary>
        private PolynomialEstimator solverD;

        /// <summary>
        /// Frame height cached for performance
        /// </summary>
        private int frameHeight;

        /// <summary>
        /// Frame height cached for performance
        /// </summary>
        private int frameWidth;

        /// <summary>
        /// Initializes a new instance of the <see cref="Ev4FloorDataPruner"/> class.
        /// </summary>
        /// <param name="maxAllowedHorizonDisplacement">Defines how much tilt up we allow (beyond this - we wont extract floor)</param>
        /// <param name="minAllowedHorizonDisplacement">Defines how much tilt down we allow (beyond this - we wont extract floor)</param>
        /// <param name="maxSurfaceRoughness">Max allowed floor roughness</param>
        /// <param name="maxVerticalTravelPercentFromCalibrated">Max allowed vertical shift up in percent as compared to calibration data</param>
        /// <param name="minVerticalTravelPercentFromCalibrated">Max allowed vertical shift down in percent as compared to calibration data</param>
        public Ev4FloorDataPruner(
            double maxAllowedHorizonDisplacement,
            double minAllowedHorizonDisplacement,
            double maxSurfaceRoughness,
            double maxVerticalTravelPercentFromCalibrated,
            double minVerticalTravelPercentFromCalibrated)
        {
            this.maxAllowedHorizonDisplacement = maxAllowedHorizonDisplacement;
            this.minAllowedHorizonDisplacement = minAllowedHorizonDisplacement;
            this.maxSurfaceRoughness = maxSurfaceRoughness;
            this.maxVerticalTravelPercentFromCalibrated = maxVerticalTravelPercentFromCalibrated;
            this.minVerticalTravelPercentFromCalibrated = minVerticalTravelPercentFromCalibrated;

            this.solverA = new PolynomialEstimator();
            this.solverB = new PolynomialEstimator();
            this.solverV = new PolynomialEstimator();
            this.solverD = new PolynomialEstimator();
        }

        /// <summary>
        /// Initializes an object with floor filtering calibration data
        /// </summary>
        /// <param name="calibrationData">An array initialized with pre-learned A and B coefficients in rows, per strip of floor (columns)</param>
        public void SetCalibrationData(double[][] calibrationData)
        {
            this.learnedFloorConstants = null;
            this.floorFilteringStripCount = 0;

            if (null != calibrationData && null != calibrationData[0])
            {
                int stripCount = calibrationData[0].Length;

                if (stripCount < 1)
                {
                    throw new InvalidOperationException("Incorrect number of floor filtering strips. Cannot be less than 1");
                }

                this.learnedFloorConstants = calibrationData;
                this.floorFilteringStripCount = stripCount;

                // We compute average B from calibration file to have a ballpark of what B is on a level robot. 
                // We use this later for outlier B filtering (i.e. when facing a wall)
                for (int i = 0; i < stripCount; ++i)
                {
                    this.calibratedBAverage += this.learnedFloorConstants[1][i];
                }

                this.calibratedBAverage /= stripCount;
                this.maxAllowedAbsoluteB = this.calibratedBAverage * (1 + this.maxVerticalTravelPercentFromCalibrated / 100);
                this.minAllowedAbsoluteB = this.calibratedBAverage * (1 - this.minVerticalTravelPercentFromCalibrated / 100);
            }
        }

        /// <summary>
        /// Determines whether or not the filter is initialized with pre-learned data
        /// </summary>
        /// <returns>Whether or not learn data has been set</returns>
        public bool IsInitialized()
        {
            return null != this.learnedFloorConstants;
        }

        /// <summary>
        /// Clears all previous state and starts a new pruning session. For performance reasons we don't want to destroy 
        /// all objects that have been initialized first time pruner was ran, but we can easily remove all data from previous session
        /// </summary>
        /// <param name="frameHeight">Frame height.</param>
        /// <param name="frameWidth">Frame width. </param>
        /// <param name="stripCount">Number of strips</param>
        public void PrepareForNewPruningSession(int frameHeight, int frameWidth, int stripCount)
        {
            this.maxAllowedHorizonPixel = frameHeight * this.maxAllowedHorizonDisplacement;
            this.minAllowedHorizonPixel = frameHeight * this.minAllowedHorizonDisplacement;

            this.solverA.Clear();
            this.solverB.Clear();
            this.solverV.Clear();
            this.solverD.Clear();

            this.frameHeight = frameHeight;
            this.frameWidth = frameWidth;

            if (0 == this.maxAllowedVarianceOfPrunedDepthStrips)
            {
                List<FastTuple<double, double>> avgStripDepths = new List<FastTuple<double, double>>();     

                int stripWidth = frameWidth / stripCount;

                int floorTestStripWidthInPixels = stripWidth / 2; 

                for (int i = 0; i < stripCount; ++i)
                {
                    int startCol = stripWidth * i + stripWidth / 2 - floorTestStripWidthInPixels / 2;
                    int endCol = startCol + floorTestStripWidthInPixels;

                    int stripCenter = (startCol + endCol) / 2;

                    avgStripDepths.Add(new FastTuple<double, double>(stripCenter, this.learnedFloorConstants[2][i]));
                }

                this.maxAllowedVarianceOfPrunedDepthStrips = this.GetNormalizedFitError(avgStripDepths) * 3;
            }
        }

        /// <summary>
        /// Add sample to pruner
        /// </summary>
        /// <param name="stripCenter">Strip center coordinate</param>
        /// <param name="floorFitResult">Strip fit coefficients</param>
        /// <param name="averageStripDepth">Average depths</param>
        public void AddSample(
            int stripCenter, 
            FastTuple<double, double, double> floorFitResult, 
            double averageStripDepth) 
        {
            this.solverA.Add(stripCenter, floorFitResult.Item1);
            this.solverB.Add(stripCenter, floorFitResult.Item2);
            this.solverV.Add(stripCenter, floorFitResult.Item3);
            this.solverD.Add(stripCenter, averageStripDepth);
        }
        
        /// <summary>
        /// Validates pruned strip data to make sure it makes sense as a whole (after pruning removes outliers)
        /// </summary>
        /// <returns>Whether or not the final A dataset of good for fitting a floor plane</returns>
        public bool IsLearnDataValid()
        {
            if (0 == this.maxAllowedVarianceOfPrunedDepthStrips)
            {
                throw new InvalidOperationException("Pruner session not initialized");
            }

            int count = this.solverD.DataPoints.Count;

            if (this.solverD.DataPoints.Count <= this.floorFilteringStripCount / MinimumGoodStripRatio)
            {
                return false;
            }

            double variance = this.GetNormalizedFitError(this.solverD.DataPoints);

            if (variance > this.maxAllowedVarianceOfPrunedDepthStrips) 
            {
                return false;
            }

            return true;
        }

        /// <summary>
        /// Prunes outlier coefficients. We apply a series of filters to make sure that what we learn is indeed floor
        /// and not just any surface
        /// </summary>
        public void PruneOutliers()
        {            
            // First we apply pre-configured absolute cutoffs. 
            this.PruneByAbsoluteValues(this.solverA.DataPoints, this.minAllowedHorizonPixel, this.maxAllowedHorizonPixel);

            this.PruneByAbsoluteValues(this.solverB.DataPoints, this.minAllowedAbsoluteB, this.maxAllowedAbsoluteB);

            this.PruneByAbsoluteValues(this.solverV.DataPoints, 0, this.maxSurfaceRoughness);

            // Prune variance by gradient. Sharp jums indicate likely scene change
            this.PruneByGradient(this.solverV.DataPoints, 3);

            // B cant vary much across the frame relative to calibrated B, so a 3 sigma spread is enough
            this.PruneByVarianceFromSpecifiedMean(this.solverB.DataPoints, this.calibratedBAverage, 2, true);

            this.PruneByVarianceFromSpecifiedMean(this.solverV.DataPoints, 0, 2, true);

            // Since datapoints have been pruned individually, we need to synchronize them
            List<List<FastTuple<double, double>>> allDatapoints = new List<List<FastTuple<double, double>>>();

            allDatapoints.Add(this.solverA.DataPoints);
            allDatapoints.Add(this.solverB.DataPoints);
            allDatapoints.Add(this.solverV.DataPoints);
            allDatapoints.Add(this.solverD.DataPoints);

            this.ReconcileDatapoints(allDatapoints);
        }

        /// <summary>
        /// Gets pruned A and B data points
        /// </summary>
        /// <param name="dataPointsA">Pruned A data points</param>
        /// <param name="dataPointsB">Pruned B data points</param>
        /// <param name="dataPointsD">Pruned Depth data points</param>
        public void GetPrunedDataPoints(out List<FastTuple<double, double>> dataPointsA, out List<FastTuple<double, double>> dataPointsB, out List<FastTuple<double, double>> dataPointsD) 
        {
            dataPointsA = this.solverA.DataPoints;
            dataPointsB = this.solverB.DataPoints;
            dataPointsD = this.solverD.DataPoints;
        }

        /// <summary>
        /// Prunes Dataset by throwing away samples that show most variance from calibrated mean
        /// </summary>
        /// <param name="dataPoints">Dataset to prune</param>
        /// <param name="calibratedMean">Calibrated mean</param>
        /// <param name="stdDevMargin">Margin to apply when pruning</param>
        /// <param name="recursivePrune">If true - prunes until no more pruning can be done</param>
        private void PruneByVarianceFromSpecifiedMean(
            List<FastTuple<double, double>> dataPoints,
            double calibratedMean,
            double stdDevMargin,
            bool recursivePrune)
        {
            int beforePruneCount = 0;

            do
            {
                beforePruneCount = dataPoints.Count;

                if (beforePruneCount == 0)
                {
                    return;
                }

                double variance = 0;

                for (int i = 0; i < beforePruneCount; ++i)
                {
                    double error = calibratedMean - dataPoints[i].Item2;

                    variance += error * error;
                }

                double stdDev = Math.Sqrt(variance / beforePruneCount);

                int index = 0;

                while (index < dataPoints.Count)
                {
                    if (Math.Abs(dataPoints[index].Item2 - calibratedMean) > stdDev * stdDevMargin)
                    {
                        dataPoints.RemoveAt(index);
                    }
                    else
                    {
                        ++index;
                    }
                }
            }
            while (recursivePrune && beforePruneCount != dataPoints.Count);
        }

        /// <summary>
        /// Reconciles individually pruned data points
        /// </summary>
        /// <param name="allDatapoints">Data point lists to reconcile</param>
        private void ReconcileDatapoints(List<List<FastTuple<double, double>>> allDatapoints)
        {
            Dictionary<double, List<FastTuple<double, double>>> reconList = new Dictionary<double, List<FastTuple<double, double>>>();

            foreach (List<FastTuple<double, double>> dataPoints in allDatapoints)
            {
                foreach (FastTuple<double, double> item in dataPoints)
                {
                    if (!reconList.ContainsKey(item.Item1))
                    {
                        reconList[item.Item1] = new List<FastTuple<double, double>>();
                    }

                    reconList[item.Item1].Add(item);
                }

                dataPoints.Clear();
            }

            int listCount = allDatapoints.Count;

            foreach (KeyValuePair<double, List<FastTuple<double, double>>> reconciledDataPoints in reconList)
            {
                if (reconciledDataPoints.Value.Count == listCount)
                {
                    for (int i = 0; i < listCount; ++i)
                    {
                        allDatapoints[i].Add(reconciledDataPoints.Value[i]);
                    }
                }
            }
        }

        /// <summary>
        /// Prunes outliers by applying pre-configured absolute thresholds
        /// </summary>
        /// <param name="dataPoints">Data set to prune</param>
        /// <param name="min">Minimum cutoff value</param>
        /// <param name="max">Maximum cutoff value</param>
        private void PruneByAbsoluteValues(List<FastTuple<double, double>> dataPoints, double min, double max)
        {
            int index = 0;

            while (index < dataPoints.Count)
            {
                double candidateValue = dataPoints[index].Item2;

                if (candidateValue > max || candidateValue < min)
                {
                    dataPoints.RemoveAt(index);
                }
                else
                {
                    ++index;
                }
            }
        }

        /// <summary>
        /// Prunes Dataset by throwing away samples that show jump in their values
        /// </summary>
        /// <param name="dataPoints">Dataset to prune</param>
        /// <param name="acceptableValueJumpGradient">Acceptable Gradient</param>
        private void PruneByGradient(List<FastTuple<double, double>> dataPoints, double acceptableValueJumpGradient)
        {
            int itemCount = dataPoints.Count;

            if (itemCount < 2)
            {
                // nothing to do for less than 2 datapoints
                return;
            }

            // First compute the average gradient, this will tell us how quickly the values jump on average
            dataPoints.Sort(this.CompareStripDataByValues);

            double avgGradient = 0;
            double previousValue = dataPoints[0].Item2;

            for (int i = 0; i < itemCount; ++i)
            {
                double currentValue = dataPoints[i].Item2;
                avgGradient += Math.Abs(currentValue - previousValue);
                previousValue = currentValue;
            }

            avgGradient /= (itemCount - 1); // -1 since there are one less gradients than there are elements in the list

            avgGradient = Math.Sqrt(avgGradient) * acceptableValueJumpGradient;

            // Now that we have the average, prune the outliers
            previousValue = dataPoints[0].Item2;

            for (int i = 0; i < itemCount; ++i)
            {
                double currentValue = dataPoints[i].Item2;
                double gradient = Math.Abs(currentValue - previousValue);

                if (gradient > avgGradient)
                {
                    dataPoints.RemoveRange(i, itemCount - i);
                    break;
                }

                previousValue = currentValue;
            }

            // Restore data to per-strip sort, which is how it was originally
            dataPoints.Sort(this.CompareStripDataByStripIndex);
        }

        /// <summary>
        /// Compares strip learn data by values
        /// </summary>
        /// <param name="x">First element</param>
        /// <param name="y">Second element</param>
        /// <returns>Returns 0 if equal, 1 if x is greater than y, -1 if x is less than y</returns>
        private int CompareStripDataByValues(FastTuple<double, double> x, FastTuple<double, double> y)
        {
            if (x.Item2 > y.Item2)
            {
                return 1;
            }
            else if (x.Item2 < y.Item2)
            {
                return -1;
            }

            return 0;
        }

        /// <summary>
        /// Compares strip learn data by strip index
        /// </summary>
        /// <param name="x">First element</param>
        /// <param name="y">Second element</param>
        /// <returns>Returns 0 if equal, 1 if x is greater than y, -1 if x is less than y</returns>
        private int CompareStripDataByStripIndex(FastTuple<double, double> x, FastTuple<double, double> y)
        {
            if (x.Item1 > y.Item1)
            {
                return 1;
            }
            else if (x.Item1 < y.Item1)
            {
                return -1;
            }

            return 0;
        }

        /// <summary>
        /// Compute Strip Distance Fit Error
        /// </summary>
        /// <param name="samples">The samples</param>
        /// <returns>Normalized standard deviation</returns>
        private double GetNormalizedFitError(List<FastTuple<double, double>> samples)
        {
            PolynomialEstimator solver = new PolynomialEstimator();

            int count = samples.Count;

            for (int i = 0; i < count; ++i)
            {
                solver.Add(samples[i].Item1, samples[i].Item2);
            }

            FastTuple<double, double, double> poly;

            bool solved = solver.FitAParabola(out poly);

            double variance = 0;
            double avgValue = 0;

            for (int i = 0; i < count; ++i)
            {
                double y = samples[i].Item1;
                double expected = PolynomialEstimator.Estimate(poly, y);

                double error = samples[i].Item2 - expected;

                avgValue += samples[i].Item2;

                variance += error * error;
            }

            variance /= count;

            avgValue /= count;

            return Math.Sqrt(variance) / avgValue;
        }
    }
}
